#!/bin/bash

# 确保脚本有执行权限
# chmod +x view_map.sh

# 设置ROS环境
source /root/catkin_ws/devel/setup.bash

# 地图目录
MAP_DIR="/root/catkin_ws/src/auto_navigation/maps"

# 显示可用的地图文件
echo "可用的地图文件:"
ls -1 $MAP_DIR/*.pcd | nl

# 如果提供了参数，使用参数作为地图文件路径
if [ $# -gt 0 ]; then
    if [[ "$1" =~ ^[0-9]+$ ]]; then
        # 如果参数是数字，则选择对应编号的地图
        MAP_NUM=$1
        MAP_FILE=$(ls -1 $MAP_DIR/*.pcd | sed -n "${MAP_NUM}p")
        if [ -z "$MAP_FILE" ]; then
            echo "错误: 没有找到编号为 $MAP_NUM 的地图文件"
            exit 1
        fi
    else
        # 否则将参数视为地图文件路径
        MAP_FILE="$1"
        # 如果不是绝对路径，则假设是相对于地图目录的路径
        if [[ ! "$MAP_FILE" = /* ]]; then
            MAP_FILE="$MAP_DIR/$MAP_FILE"
        fi
    fi
else
    # 如果没有提供参数，则提示用户选择
    echo "请输入要查看的地图文件编号:"
    read MAP_NUM
    MAP_FILE=$(ls -1 $MAP_DIR/*.pcd | sed -n "${MAP_NUM}p")
    if [ -z "$MAP_FILE" ]; then
        echo "错误: 没有找到编号为 $MAP_NUM 的地图文件"
        exit 1
    fi
fi

# 检查地图文件是否存在
if [ ! -f "$MAP_FILE" ]; then
    echo "错误: 地图文件不存在: $MAP_FILE"
    exit 1
fi

echo "正在查看地图文件: $MAP_FILE"

# 启动ROS核心（如果尚未运行）
if ! rostopic list > /dev/null 2>&1; then
    echo "启动ROS核心..."
    roscore &
    sleep 2  # 等待ROS核心启动
fi

# 在后台启动点云发布节点
echo "启动点云发布节点..."
python3 /root/catkin_ws/src/auto_navigation/scripts/view_pcd.py "$MAP_FILE" &
PUB_PID=$!

# 启动RViz进行可视化
echo "启动RViz进行可视化..."
rosrun rviz rviz -d /root/catkin_ws/src/auto_navigation/rviz/view_pcd_file.rviz

# 当RViz关闭时，终止点云发布节点
echo "清理进程..."
kill $PUB_PID

echo "查看完成。" 